#pragma once

#include <iomanip>
#include <mutex>
#include <thread>

#include <iostream>
#include <cmath>

#include "fastbev/fastbev.hpp"
#include "common/check.hpp"
#include "common/tensor.hpp"
#include "common/timer.hpp"

#include <cuda_runtime.h>
#include <string.h>
#include <vector>

#include <yaml-cpp/yaml.h>

#include <opencv2/opencv.hpp>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <visualization_msgs/MarkerArray.h> 
#include <sensor_msgs/PointCloud2.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <json.hpp>
using json = nlohmann::json;
struct Point3D {
    double x, y, z;
};


class Fastbevdetect {
  public:
    Fastbevdetect(std::string file_path) : config_file_path_(file_path){};
    ~Fastbevdetect() = default;

  protected:
    //初始化状态
    std::string config_file_path_;
    std::string visual_frame;
    std::string model;
    std::vector<std::string> json_file{"./sensor_6t/left_front.json","./sensor_6t/front.json","./sensor_6t/right_front.json",
                                        "./sensor_6t/left_rear.json","./sensor_6t/rear.json","./sensor_6t/right_rear.json"};

    ros::NodeHandle nh_;
    ros::Publisher visual_publisher;
    ros::Publisher image_publisher;

    message_filters::Subscriber<sensor_msgs::Image> *front_left_image_sub_;
    message_filters::Subscriber<sensor_msgs::Image> *front_image_sub_;
    message_filters::Subscriber<sensor_msgs::Image> *front_right_image_sub_;
    message_filters::Subscriber<sensor_msgs::Image> *back_left_image_sub_;
    message_filters::Subscriber<sensor_msgs::Image> *back_image_sub_;
    message_filters::Subscriber<sensor_msgs::Image> *back_right_image_sub_;

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image,
                                                          sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image > MySyncPolicy;
    

    std::shared_ptr<fastbev::Core> core;
    cudaStream_t stream;
    

  public:
    void Init();
    void ImageMsg2Mat(sensor_msgs::ImageConstPtr msg,cv::Mat &image);
    void HandleSensorMessage(
          const sensor_msgs::ImageConstPtr &front_left_image,
          const sensor_msgs::ImageConstPtr &front_image,
          const sensor_msgs::ImageConstPtr &front_right_image,
          const sensor_msgs::ImageConstPtr &back_left_image,
          const sensor_msgs::ImageConstPtr &back_image,
          const sensor_msgs::ImageConstPtr &back_right_image
      );
    std::shared_ptr<fastbev::Core> create_core(const std::string& model, const std::string& precision);
    std::vector<Point3D> calculateVertices(float x, float y, float z, float w, float l, float h, float yaw);
    std::vector<visualization_msgs::Marker> compute_points(std::vector<fastbev::post::transbbox::BoundingBox> bboxes);
    void visual_image(std::vector<fastbev::post::transbbox::BoundingBox> bboxes,std::vector<cv::Mat> &images);
    cv::Point2f projectPoint(const Point3D& point3D, const std::vector<double>& intrinsic, const std::vector<double>& rotation, const std::vector<double>& translation);
    cv::Mat combineImages(const std::vector<cv::Mat>& images);
    void saveImagesToBin(const std::vector<unsigned char*>& images, const std::string& filename);



};

